/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-07-05     WangJing       the first version
 */
#include "platform2D.h"

#define DBG_TAG "P2D"
#define DBG_LVL -1
#include <rtdbg.h>

#define DECLINE_SPEED 30 // 30以上会有抖动现象
#define MEDICINE_SPEED 50 // 60以上会有抖动
#define LOCA_LIST_NUM 12
#define MOVE_BIAS_X 0
#define MOVE_BIAS_Y 0

float XLocaList[X_COUNT+1] = {1414,2220,3142,3975,0};
float YLocaList[Y_COUNT+1] = {0,1400,2766,0};

static Loca2D_t CurrentLoca = {0};
static List2D_t MedicineLoca[LOCA_LIST_NUM+1] = {
        {0,0},{1,0},{2,0},{3,0},
        {0,1},{1,1},{2,1},{3,1},
        {0,2},{1,2},{2,2},{3,2},{4,3}
};

void platform2D_init(void){

    setpMotor_clearZero(2,1);
    setpMotor_clearZero(2,2);

    CurrentLoca.x = 0;
    CurrentLoca.y = 0;
}

void platform2D_MedicineLoca(int loca){
    platform2D_goList(MedicineLoca[loca].x,MedicineLoca[loca].y,MEDICINE_SPEED);
}

void platform2D_goList(int x,int y,uint16_t speed){
    Loca2D_t goLoca = {0};
    goLoca.x = XLocaList[x] + (MOVE_BIAS_X);
    goLoca.y = YLocaList[y] + (MOVE_BIAS_Y);
    platform2D_goLoca(goLoca,speed);
}

Loca2D_t platform2D_goLoca(Loca2D_t loca, uint16_t speed){
    float xDistance = fabs(loca.x - CurrentLoca.x);
    float yDistance = fabs(loca.y - CurrentLoca.y);
    float totalDistance = sqrt(xDistance * xDistance + yDistance * yDistance);

    LOG_D("xDistance:%.2f",xDistance);
    LOG_D("yDistance:%.2f",yDistance);
    LOG_D("totalDistance:%.2f",totalDistance);

    if(loca.y < CurrentLoca.y){
        speed = DECLINE_SPEED; // 下降速度
    }

    // 计算时间，以确保以相同的速度到达目标位置
    uint16_t totalTime = (uint16_t)totalDistance * 100 / speed ;
//    float xSpeed = xDistance / totalTime;
//    float ySpeed = yDistance / totalTime;

    LOG_D("totalTime:%d",totalTime);

    // 设定电机运动时间
    setpMotor_circle_time(2, 1, loca.x, totalTime);
    setpMotor_circle_time(2, 2, loca.y, totalTime);

    // 更新当前位置
    CurrentLoca.x = loca.x;
    CurrentLoca.y = loca.y;

    PLATFORM2D_DELAY(totalTime + 100);

    return CurrentLoca;
}
